#!/usr/bin/env python3

from launch import LaunchDescription
from launch_ros.actions import Node
from launch.actions import DeclareLaunchArgument, GroupAction, TimerAction
from launch.substitutions import LaunchConfiguration, PathJoinSubstitution
from launch_ros.substitutions import FindPackageShare
from launch.conditions import IfCondition

def generate_launch_description():
    # 声明启动参数
    config_file_arg = DeclareLaunchArgument(
        'config_file',
        default_value='demo_params.yaml',
        description='Configuration file name'
    )

    namespace_arg = DeclareLaunchArgument(
        'namespace',
        default_value='demo',
        description='Namespace for nodes'
    )

    start_delayed_arg = DeclareLaunchArgument(
        'start_delayed',
        default_value='false',
        description='Whether to start nodes with delay'
    )

    # 创建基础节点组
    basic_nodes = GroupAction(
        actions=[
            Node(
                package='multi_launch_demo',
                executable='publisher_node',
                name='publisher_node',
                namespace=LaunchConfiguration('namespace'),
                parameters=[PathJoinSubstitution([
                    FindPackageShare('multi_launch_demo'),
                    'config',
                    LaunchConfiguration('config_file')
                ])],
                output='screen'
            ),
            Node(
                package='multi_launch_demo',
                executable='subscriber_node',
                name='subscriber_node',
                namespace=LaunchConfiguration('namespace'),
                output='screen'
            )
        ]
    )

    # 创建服务节点组
    service_nodes = GroupAction(
        actions=[
            Node(
                package='multi_launch_demo',
                executable='service_server_node',
                name='service_server_node',
                output='screen'
            ),
            Node(
                package='multi_launch_demo',
                executable='service_client_node',
                name='service_client_node',
                output='screen'
            )
        ]
    )

    # 创建参数节点
    param_node = Node(
        package='multi_launch_demo',
        executable='param_node',
        name='param_node',
        parameters=[PathJoinSubstitution([
            FindPackageShare('multi_launch_demo'),
            'config',
            LaunchConfiguration('config_file')
        ])],
        output='screen'
    )

    # 创建带延迟启动的节点
    delayed_nodes = TimerAction(
        period=5.0,
        actions=[
            Node(
                package='multi_launch_demo',
                executable='publisher_node',
                name='delayed_publisher_node',
                parameters=[{
                    'publish_frequency': 0.5,
                    'message_prefix': 'Delayed'
                }],
                output='screen'
            )
        ],
        condition=IfCondition(LaunchConfiguration('start_delayed'))
    )

    return LaunchDescription([
        config_file_arg,
        namespace_arg,
        start_delayed_arg,
        basic_nodes,
        service_nodes,
        param_node,
        delayed_nodes
    ])
